#include <TrajectoryPlanner.h>

TrajectoryPlanner::TrajectoryPlanner(){
	ros::NodeHandle n;
	cost_marker_pub = n.advertise<visualization_msgs::Marker>("control_marker", 10);
}

TrajectoryPlanner::TrajectoryPlanner(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt){
	init( simulation_time, xv_samples, tv_samples, cx, cy, ct, cvl, cvt );
}

void TrajectoryPlanner::init(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt, fnavfn::Path::Request &req){
	P.initialize(req);
	init(simulation_time,xv_samples,tv_samples,cx,cy,ct,cvl,cvt);
}

void TrajectoryPlanner::init(double simulation_time, int xv_samples, int tv_samples, double cx, double cy, double ct, double cvl, double cvt){
	this->xv_samples = xv_samples;
	this->tv_samples = tv_samples;
	dt = 0.1f;
	sim_time = ceil( simulation_time / dt );	
	
	/** Weighing constants for trajectory evaluation **/
	std::ifstream ifs("config/weights.config");
	if (!ifs){
		ROS_ERROR( "Can not open config/weights.config" );
		return;
	}else{
		ifs >> alpha >> beta >> gamma >> delta;
	}
	fe.init(cx,cy,ct,cvl,cvt);
}

ControlCommand TrajectoryPlanner::get_best_trajectory( ){
	//ROS_INFO("Best trajectory needed");
	double dvt = 2.0f * fe.max_ang_velocity() / (double)tv_samples;
	mapper.updateMapOdomTransform();
	Trajectory best_trajectory;
	ControlCommand best_ctrl;
	double best_cost = INF;
	
	for( double av = -fe.max_ang_velocity(); av <= fe.max_ang_velocity(); av += dvt ){
		_for_all_linear_velocities(av, best_trajectory, best_ctrl, best_cost);		
	}
	traj.clearViz();
	std::cout << best_ctrl.lv() << " " << best_ctrl.av() << " " << best_cost << std::endl;
	return best_ctrl;
}

void TrajectoryPlanner::_for_all_linear_velocities(double av,Trajectory &btraj, ControlCommand &best_ctrl, double &best_cost ){
	/// There is no velocity along the ycomponent. There is only the linear and angular velocities.
	double dvx = fe.max_lin_velocity() / (double)xv_samples;
	double lv;
	for( lv = 0; lv <= fe.max_lin_velocity(); lv += dvx ){
		ControlCommand ctrl(lv,av,sim_time);
		traj.clear();
		if( _generateTrajectory(traj, ctrl) ){
			//std::cout<< ctrl.lv() << " " << ctrl.av() << " " << traj.get_cost() << std::endl;
			if( best_cost > traj.get_cost() ){
				best_cost = traj.get_cost();
				best_ctrl = ctrl;
			}
		}
	}
	//std::cout << traj.get_cost() << std::endl;
}

bool TrajectoryPlanner::_generateTrajectory(Trajectory &tr, ControlCommand &cmd ){
	double x = fe.x(), y = fe.y(), t = fe.theta();
	double vx = fe.lv(), vt = fe.av();
	double cvx = cmd.lv(), cvt = cmd.av();
	double ct = 0.0, qr, res = P.grain, total_obs_cost = 0.0;
	
	for( int i=0; i< cmd.number_steps(); ++i){
		ct+=dt;		// update time step
		
		vx = update_velocity(vx,cvx,fe.max_lin_acc(),dt);	//update linear velocity
		vt = update_velocity(vt,cvt,fe.max_ang_acc(),dt);	//update angular velocity
		
		x = update_x(x,vx,t,dt);
		y = update_y(y,vx,t,dt);
		t = update_t(t,vt,dt);
		
		qr = interpolate_obs_cost(x,y,res);
		total_obs_cost += alpha * qr;
		
		if( qr > (double)INF/100.0 ){
			printf("INF ");
			return false;
		}
		tr.add_point(x,y,t);	// update pose
	}
	double howCloseToGoal;
	double pdist = gamma * calculate_pdist(x,y,howCloseToGoal);
	double gdist = beta * ( howCloseToGoal / (double) P.pathx.size() ); //interpolate_gdist_cost(x,y,res);

	double disp = distance(tr.get_point(0),tr.get_point(tr.length()-1));
	disp = disp == 0.0 ? 0.000001f : disp;
	double vel = delta * ct * ct / disp ;	/// @brief twisted way of writing 1/(v*v)
	tr.add_cost( total_obs_cost + gdist + pdist + vel );
	printf("%3.2lf %3.2lf<%lf %lf %lf %lf>\n", cvx, cvt, total_obs_cost, gdist, pdist, tr.get_cost());
	tr.plot();
	return true;
}

double TrajectoryPlanner::distance(TFFF from,TFFF to){
	double fx = from.second.first, fy = from.second.second;
	double tx = to.second.first, ty = to.second.second;
	//printf("[%lf %lf->%lf %lf] ",fx,fy,tx,ty);
	double result = (fx-tx)*(fx-tx) + (fy-ty)*(fy-ty);
	return result;
}

double TrajectoryPlanner::interpolate_obs_cost(double xx, double yy, double res){
	mapper.point_in_map(xx,yy);
	PII mapIndex = index(xx,yy,res);

	int intx = mapIndex.first;
	int inty = mapIndex.second;

/**
	return interpolate_at(
		yy,xx,qy,qy+res,qx,qx+res,
		P.obs[intx+1][inty],P.obs[intx][inty],
		P.obs[intx+1][inty+1],P.obs[intx][inty+1]
	);
**/

	return P.getObstacleCostAt(intx,inty);
}

double TrajectoryPlanner::interpolate_gdist_cost(double xx, double yy, double res){
	mapper.point_in_map(xx,yy);

	PII mapIndex = index(xx,yy,res);
	int intx = mapIndex.first;
	int inty = mapIndex.second;
/**
	double x = interpolate_at(
		yy,xx,qy,qy+res,qx,qx+res,
		P.gdist[intx+1][inty],P.gdist[intx][inty],
		P.gdist[intx+1][inty+1],P.gdist[intx][inty+1]
	);
**/
	double gdist = P.gdist[intx][inty];
	return gdist < INF/100.0f ? gdist : INF/10000.0f;
}

double TrajectoryPlanner::calculate_pdist(double fx, double fy, double &howCloseToGoal){
	mapper.point_in_map(fx,fy);
	double tx, ty, min=INF, d, mx, my;
	for(int i=0;i<P.pathx.size();i++){
		tx = P.pathx[i];
		ty = P.pathy[i];
		d = (fx-tx)*(fx-tx) + (fy-ty)*(fy-ty);
		if( d < min ){
			min = d;
			mx = tx;
			my = ty;
			howCloseToGoal = P.pathx.size() - i;
		}
	}
	cm.PlotPoses(my/0.02,mx/0.02,cost_marker_pub,0.02);
	//std::cout << "ends at " << fx << " " << fy << "closest point on map is @ "<< d << " " << mx << " " << my << std::endl;
	return (min);
}

